#include "app_tentacle.h"
#include "app.h"

// 触角控制使用示例

// 示例1：基本位置控制
void Tentacle_Example_Position_Control(void)
{
    // 初始化触角控制
    Tentacle_Init();
    
    // 使能触角控制
    Tentacle_Enable(1);
    
    // 设置目标位置 (x, y, z) 单位：mm
    Tentacle_Set_Target_Position(30.0f, 20.0f, 80.0f);
    
    // 设置位置控制模式
    Tentacle_Set_Mode(TENTACLE_MODE_POSITION);
    
    // 触角会自动移动到目标位置
    // 在Motion_Handle()中会调用Tentacle_Control_Handle()进行控制
}

// 示例2：圆形运动
void Tentacle_Example_Circle_Motion(void)
{
    // 初始化触角控制
    Tentacle_Init();
    
    // 使能触角控制
    Tentacle_Enable(1);
    
    // 设置圆形运动模式
    Tentacle_Set_Mode(TENTACLE_MODE_CIRCLE);
    
    // 触角会按照圆形轨迹运动
    // 圆形半径和高度在初始化时设置
}

// 示例3：螺旋运动
void Tentacle_Example_Spiral_Motion(void)
{
    // 初始化触角控制
    Tentacle_Init();
    
    // 使能触角控制
    Tentacle_Enable(1);
    
    // 设置螺旋运动模式
    Tentacle_Set_Mode(TENTACLE_MODE_SPIRAL);
    
    // 触角会按照螺旋轨迹运动
}

// 示例4：轨迹跟踪
void Tentacle_Example_Trajectory_Tracking(void)
{
    // 初始化触角控制
    Tentacle_Init();
    
    // 使能触角控制
    Tentacle_Enable(1);
    
    // 设置轨迹跟踪模式
    Tentacle_Set_Mode(TENTACLE_MODE_TRAJECTORY);
    
    // 生成自定义轨迹
    trajectory_point_t custom_trajectory[20];
    Tentacle_Generate_Line_Trajectory(0, 0, 50,    // 起点
                                     50, 50, 100,  // 终点
                                     20,           // 轨迹点数
                                     custom_trajectory);
    
    // 设置轨迹
    // 注意：这里需要修改触角控制代码来支持自定义轨迹
}

// 示例5：触角控制状态查询
void Tentacle_Example_Status_Query(void)
{
    // 获取当前触角位置
    float current_x = g_tentacle_ctrl.current.x;
    float current_y = g_tentacle_ctrl.current.y;
    float current_z = g_tentacle_ctrl.current.z;
    
    // 获取目标位置
    float target_x = g_tentacle_ctrl.target.x;
    float target_y = g_tentacle_ctrl.target.y;
    float target_z = g_tentacle_ctrl.target.z;
    
    // 计算位置误差
    float error = Tentacle_Calculate_Distance(current_x, current_y, current_z,
                                             target_x, target_y, target_z);
    
    // 检查是否到达目标位置
    if (error < 5.0f) {  // 5mm容差
        DEBUG("Tentacle reached target position\n");
    }
    
    // 检查位置是否可达
    if (Tentacle_Is_Position_Reachable(100, 100, 100)) {
        DEBUG("Position (100, 100, 100) is reachable\n");
    } else {
        DEBUG("Position (100, 100, 100) is not reachable\n");
    }
}

// 示例6：触角控制参数调整
void Tentacle_Example_Parameter_Adjustment(void)
{
    // 调整最大运动速度
    g_tentacle_ctrl.max_speed = 100.0f;  // 100mm/s
    
    // 调整圆形运动半径
    g_tentacle_ctrl.circle_radius = 50.0f;  // 50mm
    
    // 调整螺旋运动参数
    g_tentacle_ctrl.spiral_radius = 30.0f;   // 30mm
    g_tentacle_ctrl.spiral_height = 150.0f;  // 150mm
    
    // 调整轨迹时间
    g_tentacle_ctrl.trajectory_time = 10.0f;  // 10秒
}

// 示例7：触角安全控制
void Tentacle_Example_Safety_Control(void)
{
    // 检查触角是否在安全范围内
    if (g_tentacle_ctrl.current.x < -100 || g_tentacle_ctrl.current.x > 100 ||
        g_tentacle_ctrl.current.y < -100 || g_tentacle_ctrl.current.y > 100 ||
        g_tentacle_ctrl.current.z < 0 || g_tentacle_ctrl.current.z > 200) {
        
        // 触角超出安全范围，立即停止
        Tentacle_Stop();
        DEBUG("Tentacle out of safety range, stopped\n");
    }
    
    // 检查线长度是否在安全范围内
    if (g_tentacle_ctrl.current.line1_length > TENTACLE_MAX_LINE_LENGTH ||
        g_tentacle_ctrl.current.line2_length > TENTACLE_MAX_LINE_LENGTH ||
        g_tentacle_ctrl.current.line3_length > TENTACLE_MAX_LINE_LENGTH) {
        
        // 线长度超出安全范围，立即停止
        Tentacle_Stop();
        DEBUG("Tentacle line length out of safety range, stopped\n");
    }
}
